/* 
 * File:   anglecons.hpp
 * Author: posypkin
 *
 * Created on July 23, 2012, 12:48 PM
 */

#include <math.h>
#include <problems/cover/common/lipzobjective.hpp>


#ifndef ANGLECONS_HPP
#define	ANGLECONS_HPP

template <class FT> class AngleCons : public LipzObjective<FT> {
public:

    /**
     * The constructor
     * @param nsec number of sections in manipulator
     * @param bound right hand side
     * @param sign the sign (1 for <=, -1 for >=)
     */
    AngleCons(int nsec, FT bound, FT sign) {
        mNSec = nsec;
        mBound = bound;
        mSign = sign;
    }

    FT func(const FT* x) {
        FT phi = 0;
        for (int i = 0; i < mNSec; i++) {
            phi += x[i];
        }
        return mSign * (phi - mBound);
    }

    FT getFuncLipzConstant(const FT* a, const FT* b) {
        return sqrt((double)mNSec);
    }

private:
    int mNSec;
    FT mBound;
    FT mSign;
};

#endif	/* ANGLECONS_HPP */

